#pragma config(Sensor, in1,    RgihtLine,           sensorLineFollower)
#pragma config(Sensor, in2,    MidLine,             sensorLineFollower)
#pragma config(Sensor, in3,    LeftLine,            sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Motor,  port1,           LeftLift1,     tmotorNormal, openLoop)
#pragma config(Motor,  port2,           DriveL,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL2,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           DriveR2,       tmotorNormal, openLoop)
#pragma config(Motor,  port6,           Gate1,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           Gate2,         tmotorNormal, openLoop)
#pragma config(Motor,  port10,          LeftLift2,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* DRIVE FUNCTIONS
* drive_forward_1msec
* drive_straight
* drive_straight_to_touch
*/

/* drive_forward_msec
* @purpose: autonomously drive robot forward for provided duration
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param duration: duration in milliseconds
*/

void drive_forward_msec(int speed, int duration) {
  motor[DriveR] = speed;
  motor[DriveL] = speed;
  motor[DriveL2] = speed;
  motor[DriveR2] = speed;
  wait1Msec(duration);
  allMotorsOff();
}
/* drive_straight
* @purpose: autonomously drive robot forward for provided distance in inches
* precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param distance: distance in inches
* TODO: consider adding an acceleration loop
*///there are 360 ticks in an encoder revolution
void drive_straight(int speed, float distance) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while( ( (float) SensorValue[EncoderL]  * 4 / 360 * PI) < distance &&
    ( (float) SensorValue[EncoderR] * 4 / 360 * PI) < distance) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL] = speed -1;
      motor[DriveL2] = speed -1;
      motor[DriveR2] = speed;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed -1;
      motor[DriveL] = speed;
      motor[DriveL2] = speed;
      motor[DriveR2] = speed -1;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL] = speed;
      motor[DriveL2] = speed;
      motor[DriveR2] = speed;
    }
  }
  allMotorsOff();
}

/* drive_straight_unlocked
* @purpose: autonomously drive robot forward for provided distance in inches WHILE allowing
* precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param distance: distance in inches
* TODO: consider adding an acceleration loop
*///there are 360 ticks in an encoder revolution
void drive_straight_unlocked(int speed, float distance) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  if( ( (float) abs(SensorValue[EncoderL])  * 2 / 360 * PI) < distance &&
    ( (float) abs(SensorValue[EncoderR]) * 2 / 360 * PI) < distance) {
    if(abs(SensorValue[EncoderL]) > abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed + 1;
    }
    else if(abs(SensorValue[EncoderL]) < abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed + 1;
      motor[DriveL]  = -speed;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed;
    }
  }
}

/* drive_straight_to_touch
* @purpose: autonomously drive robot forward until touch sensors are triggered
* precondition: 2 touch sensors, left and right
* @param speed: speed of motors, -127 to 127; negative values go reverse
*/
/*void drive_straight_to_touch(int speed) {
while(SensorValue[LeftTouch]  == 0 || SensorValue[RightTouch] == 0) {
if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
motor[DriveR] = speed;
motor[DriveL]  = speed - 1;
}
else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
motor[DriveR] = speed - 1;
motor[DriveL]  = speed;
}
else {
motor[DriveR] = speed;
motor[DriveL]  = speed;
}
}
motor[DriveL] = 0;
motor[DriveR]= 0;
}*/

/* turn
* @purpose: autonomously execute a point turn
* @param speed: speed of motors, 0 to 127; no negative values
* @param degrees: number of degrees to turn
* TODO: complete this function
*/
void turn(int speed, int degrees) {
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;
  while(SensorValue[EncoderL] < degrees || SensorValue[EncoderR] < degrees*2) {
    motor[DriveL] = speed;
    motor[DriveR] = -speed;
  }
  motor[DriveL] = 0;
  motor[DriveR] = 0;
}

void gate(int speed) {
  motor[Gate1] = speed;
  motor[Gate2] = speed;
  wait1Msec(2000);
  motor[Gate1] = 0;
  motor[Gate2] = 0;
}

void lift(int speed) {
  motor[LeftLift1] = speed;
  motor[LeftLift2] = speed;
  wait1Msec(2000);
  motor[LeftLift1] = 0;
  motor[LeftLift2] = 0;
}

/*void turn_msec(int speed, int duration) {

if(){
motor[DriveL]=-speed;
motor[DriveR]=speed;
wait1Msec(duration);
motor[DriveL]=0;
motor[DriveR]=0;}
else{
motor[DriveL]=speed;
motor[DriveR]=-speed;
wait1Msec(duration);
motor[DriveL]=0;
motor[DriveR]=0;}
}*/

/* MODE SELECTION FUNCTION
* mode_select
* @purpose use a potentiometer to set mode
* high potentiometer values return 1, low potentiometer values return 0
*/

/*int mode_select() {
if(SensorValue[Mode] >= 2000)
return 1;
else
return 0;
}*/


/* LINE FOLLOWER FUNCTIONS
* follow_line_msec
* follow_line_distance
*/

/*follow_line_msec
* @purpose follow a line for provided duration using 3 light sensors
* @param speed: speed of motors, 0 to 127
* @param duration: duration of line following in milliseconds
*/
/*void follow_line_msec(int speed, int duration) {
ClearTimer(T1);
int threshold = 250;  //guessed threshold; low numbers are lighter
while(time1[T1] < duration) {
if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
motor[DriveL]  = speed;
motor[DriveR] = speed;
}
else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
motor[DriveL]  = speed - 1;
motor[DriveR] = speed;
}
else if(SensorValue[RightLine] < threshold) { //right sensor is on line
motor[DriveL]  = speed;
motor[DriveR] = speed - 1;
}
}
}*/


/*follow_line_distance
* @purpose follow a line for provided duration using 3 light sensors
* precondition: 4-inch wheels
* @param speed: speed of motors, 0 to 127
* @param distance: distance to travel in inches
*/
void follow_line_distance(int speed, float distance) {
int threshold = 250;  //guessed threshold; low numbers are lighter
while(((float)SensorValue[EncoderL]+(float)SensorValue[EncoderR])/2 * 4 / 360 * PI < distance){
if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
       motor[DriveR] = speed;
  motor[DriveL] = speed;
  motor[DriveL2] = speed;
  motor[DriveR2] = speed;
}
else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
       motor[DriveR] = speed;
  motor[DriveL] = speed-1;
  motor[DriveL2] = speed-1;
  motor[DriveR2] = speed;
}
else if(SensorValue[RightLine] < threshold) { //right sensor is on line
       motor[DriveR] = speed-1;
  motor[DriveL] = speed;
  motor[DriveL2] = speed;
  motor[DriveR2] = speed-1;
}
}
}

int programselect = 1;
bool redteam = true;
bool programselecting = true;
const int totalprogramnumber = 5;



//function:

void screenrefresh()
{
  clearLCDLine(1);
  displayLCDPos(1,0);
  if (redteam == true)
  {
    displayNextLCDString("RED ");
  }
  if (redteam == false)
  {
    displayNextLCDString("BlUE");
  }
  displayNextLCDString("    ");
  displayNextLCDNumber(programselect);
  displayNextLCDString("     OK");
}
